/** ###################################################################
**     Filename  : EM.c
**     Project   : parking
**     Processor : MC9S12XS128MAL
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 2013-3-9, 11:09
**     Contents  :
**         User source code
**
** ###################################################################*/

/* MODULE EM */

#include "EM.h"
#include "Drv.h"
#include "Rtl.h"
#include <math.h>


U8 EMLeftFrontValue = EM_VALUE_DEFAULT;
U8 EMRightFrontValue = EM_VALUE_DEFAULT;
U8 EMLeftBackValue = EM_VALUE_DEFAULT;
U8 EMRightBackValue = EM_VALUE_DEFAULT;
U8 EMpLeftFrontTempValue = EM_VALUE_DEFAULT;
U8 EMpRightFrontTempValue = EM_VALUE_DEFAULT;
U8 EMpLeftBackTempValue = EM_VALUE_DEFAULT;
U8 EMpRightBackTempValue = EM_VALUE_DEFAULT;
U8 EMpReadCount = 0;
S16 EMCorrectionFront = EM_CORRECTION_DEFAULT_FRONT;
S16 EMCorrectionBack = EM_CORRECTION_DEFAULT_BACK;

U16 EMDirHistory[EM_HISTORY_LEN];
volatile bool EMDetecting = FALSE;

void EMStart(void){
	while(AD_Start() != ERR_OK){}
	EMpLeftFrontTempValue = EM_VALUE_DEFAULT;
	EMpRightFrontTempValue = EM_VALUE_DEFAULT;
	EMpLeftBackTempValue = EM_VALUE_DEFAULT;
	EMpRightBackTempValue = EM_VALUE_DEFAULT;
	EMpReadCount = 0;	
}

void EMStop(void){
	while(AD_Stop() != ERR_OK){}
}



void EMReadValue(void){
	U8 lfv, rfv, lbv, rbv;
	if ((AD_GetChanValue8(EM_LEFT_FRONT_CHANNEL, &lfv) == ERR_OK)
		&&(AD_GetChanValue8(EM_RIGHT_FRONT_CHANNEL, &rfv) == ERR_OK)
		&&(AD_GetChanValue8(EM_LEFT_BACK_CHANNEL, &lbv) == ERR_OK)
		&&(AD_GetChanValue8(EM_RIGHT_BACK_CHANNEL, &rbv) == ERR_OK)
){
		if (lfv > EMpLeftFrontTempValue){
			EMpLeftFrontTempValue = lfv;	
		}
		if (rfv > EMpRightFrontTempValue){
			EMpRightFrontTempValue = rfv;	
		}
		if (lbv > EMpLeftBackTempValue){
			EMpLeftBackTempValue = lbv;	
		}
		if (rbv > EMpRightBackTempValue){
			EMpRightBackTempValue = rbv;	
		}
		++EMpReadCount;
	}
	if (EMpReadCount > EM_READ_COUNT_MAX){
		EMLeftFrontValue = EMpLeftFrontTempValue;
		EMRightFrontValue = EMpRightFrontTempValue;
		EMLeftBackValue = EMpLeftBackTempValue;
		EMRightBackValue = EMpRightBackTempValue;
		
		//has been moved into EMStart
		/*
		EMpLeftFrontTempValue = EM_VALUE_DEFAULT;
		EMpRightFrontTempValue = EM_VALUE_DEFAULT;
		EMpReadCount = 0;
		*/
		EMStop();
		EMDetecting = FALSE;	
	}
}

void EMIrqHandler(void){   
	EMReadValue();		
}

//right - left
S16 EMGetValueFront(void){
	return ((S16)EMRightFrontValue) - ((S16)EMLeftFrontValue) + EMCorrectionFront;	
}

S16 EMGetValueBack(void){
	S16 dir;
	dir = ((S16)EMRightBackValue) - ((S16)EMLeftBackValue);
	
	if (EMRightBackValue < EM_DETECT_THRESHOLD){
		dir *= 3;
		dir /= 2;		
	}
	if (EMLeftBackValue < EM_DETECT_THRESHOLD){
		dir *= 3;
		dir /= 2;
	}
	return 	dir + EMCorrectionBack;	
}

static void EMDetect(void){
	EMDetecting = TRUE;
	EMStart();
	while(EMDetecting){}	
}

void EMDriveForward(void){
	S16 dir;
	U16 duty;
	EMDetect();
	dir = EMGetValueFront();
	duty = RtlLimitU16((U16)(((S16)DrvServoMiddle) + dir * EM_DIR_SCALE_FRONT), DrvServoLeft, DrvServoRight);
	DRV_SERVO_SetDuty(EMDirFilter(duty));
}

void EMInit(void){
	int i = 0;
	for (i = 0; i < EM_HISTORY_LEN; ++i){
		EMDirHistory[i] = DrvServoMiddle; 		
	}
}

U16 EMDirFilter(U16 dir){
	//return dir;
	static int pos = 0;
	int i; 
	U32 sum = 0;
	EMDirHistory[pos++] = dir;
	if (pos >= EM_HISTORY_LEN){
		pos = 0;	
	}
	for (i = 0; i < EM_HISTORY_LEN; ++i){
		sum += EMDirHistory[i];	
	}
	sum += DrvServoMiddle;
	return (U16)(sum / (EM_HISTORY_LEN + 1));	   
		
}

void EMDriveBackward(void){
	S16 dir;
	S16 fdir;
	S16 slope;
	U16 duty;
//	static U8 state = 0;
	EMDetect();
	dir = EMGetValueBack();
	fdir = EMGetValueFront();
	slope = dir - fdir + EM_DIR_SLOPE_CORRECTION;
	
	if (RtlAbsS16(slope) >= EM_DIR_SLOPE_THRESHOLD){
		duty = RtlLimitU16(
			(U16)((S16)DrvServoMiddle + slope * EM_DIR_SLOPE_SCALE)
		, DrvServoLeft, DrvServoRight);
	}else{
		duty = 	RtlLimitU16(
		(U16)((S16)DrvServoMiddle + dir * EM_DIR_SCALE_BACK)
		, DrvServoLeft, DrvServoRight);
	}
/*	if (state ==0){
		if (RtlAbsS16(dir) <= EM_DIR_MIDDLE_THRESHOLD){
			state = 1;	
		}
	}else if (state == 1){
		if (RtlAbsS16(fdir) <= EM_DIR_MIDDLE_FRONT_THRESHOLD){
			state = 0;	
		}
	}	
	duty = RtlLimitU16((U16)(((S16)DrvServoMiddle)
		+ (state == 0? dir * EM_DIR_SCALE_BACK: -fdir * EM_DIR_SCALE_FRONT))
		, DrvServoLeft, DrvServoRight);		 */
		
/*		
	duty = RtlLimitU16((U16)((S16)DrvServoMiddle + dir * EM_DIR_SCALE_BACK), DrvServoLeft, DrvServoRight);*/
//	DRV_SERVO_SetDuty(EMDirFilter(duty));  
	DRV_SERVO_SetDuty(duty);    
  
}





/* END EM */
